#ifndef _BM_PRIMITIVE
#define _BM_PRIMITIVE

#include <boost/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <iomanip>

template <typename Value> class BooleanMeshVertex ; 
template <typename Value> class BooleanMeshFace ; 
template <typename Value> class BooleanMeshHalfEdge ; 
template <typename Value> class BooleanMeshEdge ; 
template <typename Value> class BooleanMesh;
template <typename Value> class IntersectionRecord ;

extern const double tiny_value ;
extern const double sq_tiny_value ;
//const double tiny_value = 1e-5 ;
extern const double large_value  ;

extern std::vector<std::deque<Vector <double> > > v_curve; 
// This structure is used for merging intersection. 
extern std::map <std::pair <unsigned, unsigned>, IntersectionRecord<Value> > map_mergeInter;
	 


template <typename Value>
struct BooleanMeshPrimitive : public MeshPrimitive <Value>
{
	typedef Value            ValueType ; 
	typedef BooleanMeshFace <Value>     FaceType ;
	typedef BooleanMeshVertex <Value>   VertexType  ;
	typedef BooleanMeshHalfEdge <Value>   HalfEdgeType ; 
	typedef BooleanMeshEdge <Value>     EdgeType ;
	typedef Vector<Value>    VectorType ;
	typedef VectorType       PointType ;
	typedef BooleanMesh<BooleanMeshPrimitive<Value> > MeshType ;

};


template <typename PrimitivesType> class BooleanMesh ;

template <typename Value>
class BooleanMeshHalfEdge : public HalfEdge<Value>
{
public :

    BooleanMeshHalfEdge() 
		: HalfEdge<Value>(), mb_isallocated(false), adjacent(NULL)  {}

protected :
	typedef typename BooleanMeshPrimitive<Value>::VectorType   VectorType ;
	typedef typename BooleanMeshPrimitive<Value>::PointType    PointType ;
	typedef typename BooleanMeshPrimitive<Value>::HalfEdgeType HalfEdgeType ; 
	typedef typename BooleanMeshPrimitive<Value>::MeshType     MeshType ;

public :
	bool is_allocated () { return mb_isallocated;}
	void set_allocate (bool _v = true ) {this->mb_isallocated = _v;}
	//void set_point_idx (int idx) { m_intr_point_idx = idx; } 
	//int get_point_idx () const {return m_intr_point_idx ;}
public :
	PointType m_intr_point ;
	bool mb_isallocated ;

	HalfEdgeType * adjacent ;
	MeshType *  mp_mesh; 
	friend ostream & operator << (ostream & ostr, BooleanMeshHalfEdge<Value> * he)
	{
		ostr<<he->from_v->m_coord<<' '<<he->next->from_v->m_coord<<' ';
		return ostr ;
	}

	// Array of the previous m_intr_point ;
	std::vector<PointType> m_points; 
};

template <typename Value>
struct IntersectionRecord
{
	typedef typename BooleanMeshPrimitive<Value>::HalfEdgeType HalfEdgeType ; 
	
	PRIMITIVE_TYPE type ;
	Segment <Value> segment ;
	Polygon <Value> polygon ;
	std::vector<std::pair<HalfEdgeType *, HalfEdgeType *> > v_merge_he; 
};

template <typename Value> 
class BooleanMeshEdge  : public Edge<Value>
{
protected:
	typedef typename BooleanMeshPrimitive<Value>::ValueType    ValueType; 
	typedef typename BooleanMeshPrimitive<Value>::FaceType     FaceType;
	typedef typename BooleanMeshPrimitive<Value>::VertexType   VertexType;
	typedef typename BooleanMeshPrimitive<Value>::HalfEdgeType HalfEdgeType ; 
	typedef typename BooleanMeshPrimitive<Value>::EdgeType     EdgeType ; 
	typedef typename BooleanMeshPrimitive<Value>::VectorType   VectorType ;
	typedef typename BooleanMeshPrimitive<Value>::PointType    PointType ;

public :
	// Record the new created vertices on two meshes.
	// Build their corresponding relationship. 
	std::vector<VertexType *> v_intr_point; 
	std::vector<std::pair<PointType, VertexType *> > v_edge_point ;

	std::vector <std::pair<PointType, unsigned> > v_point; 
private :

};
template <typename Value>
class BooleanMeshVertex : public Vertex<Value>
{
protected :
	typedef typename BooleanMeshPrimitive<Value>::ValueType    ValueType; 
	typedef typename BooleanMeshPrimitive<Value>::FaceType     FaceType;
	typedef typename BooleanMeshPrimitive<Value>::VertexType   VertexType;
	typedef typename BooleanMeshPrimitive<Value>::HalfEdgeType HalfEdgeType ; 
	typedef typename BooleanMeshPrimitive<Value>::EdgeType     EdgeType ; 
	typedef typename BooleanMeshPrimitive<Value>::VectorType   VectorType ;
	typedef typename BooleanMeshPrimitive<Value>::PointType    PointType ;
	typedef typename BooleanMeshPrimitive<Value>::MeshType     MeshType ;

	typedef typename Vertex<Value>::VVIter VVIter ;
public :
    /**
	 * Calculate the area of local averaging region. 
	 */
	const ValueType get_local_area ()  const 
	{
		ValueType area = ValueType(0);
		PointType center (ValueType(0), ValueType(0), ValueType(0));
		unsigned count = 0; 


		for (VVIter iter = BooleanMeshVertex<Value>::vv_begin(); iter != BooleanMeshVertex::vv_end(); ++iter)
		{
			//Fix me later. I will define a new class Polygon. 
			center += iter->m_coord ;
			++count ;
		}
		center /= ValueType(count); 
		
		return this->m_local_area ; 
	}

	MeshType *  mp_mesh;
private :
	Value m_local_area ;

};

/**
 * This class extend the IntersectResult. 
 * It records the intersection info for constructing new faces. 
 */
template <typename Value>
struct BooleanMeshIntersectResult : public IntersectResult<Value>
{
	typedef typename BooleanMeshPrimitive<Value>::HalfEdgeType HalfEdgeType ; 
	typedef BooleanMesh<Value>  MeshType; 

	// One intersection point is on one halfedge. No duplication is permitted. 
	std::vector<HalfEdgeType *> v_he; 	

	HalfEdgeType * source_he ;
	HalfEdgeType * target_he ;


	std::vector<std::vector<Vector<Value> > > cut_set ;
	std::vector<std::vector<Vector<Value> > > opposite_cut_set ;
	std::vector<std::pair<HalfEdgeType * , HalfEdgeType * > > he_set; 
	std::vector<std::pair<HalfEdgeType * , HalfEdgeType * > > opposite_he_set; 
};


/**
 * Extend the Face class
 */
template <typename Value>
class BooleanMeshFace : public Face<Value>
{

public:
	typedef typename BooleanMeshPrimitive<Value>::ValueType    ValueType; 
	typedef typename BooleanMeshPrimitive<Value>::FaceType     FaceType;
	typedef typename BooleanMeshPrimitive<Value>::VertexType   VertexType;
	typedef typename BooleanMeshPrimitive<Value>::HalfEdgeType HalfEdgeType ; 
	typedef typename BooleanMeshPrimitive<Value>::EdgeType     EdgeType ; 
	typedef typename BooleanMeshPrimitive<Value>::VectorType   VectorType ;
	typedef typename BooleanMeshPrimitive<Value>::PointType    PointType ;
	typedef typename FaceType::FHeIter                         FHeIter; 
	typedef Face<Value>                                        ParentType ;
	typedef Polygon<Value>                                     PolygonType ;

public :
	typedef typename FaceType::FVIter                          FVIter; 
	// Array of previous points. 
	std::vector<std::pair<PointType, VertexType *> > v_face_point ;

public:

	BooleanMeshFace () : Face<Value>(), accessed (false) {}

	/**
	 * Convert the face to an oriented polygon. 
	 * @param polyogn polygon. 
	 */
	void Face2Polygon (PolygonType & _polygon)
	{
		for (FVIter iter = this->fv_begin(); iter != this->fv_end(); ++iter)
		{
			_polygon.append_vertex( (*iter)->m_coord);
		}
		
	}

	void Face2Halfedge (std::vector<HalfEdgeType *> & v_he)
	{
		v_he.clear();
		for (FHeIter iter = this->fhe_begin(); iter != this->fhe_end(); ++iter)
		{
			v_he.push_back((HalfEdgeType *) (*iter));
		}
	}

	const VectorType & operator [](unsigned _id) const
	{
		return this->m_polygon[_id] ;
	}

	
	unsigned size() const
	{
		return this->m_polygon.size();
	}
	
	/**
	 * Get the face polygon. 
	 */
	const PolygonType & get_polygon () const 
	{
		return this->m_polygon ;
	}

	/**
	 * This function will be called when constructing the mesh.
	 */
	void construct()
	{
		ParentType::construct();
		// Construct the polygon
		m_polygon.clear();
		for (FVIter iter = this->fv_begin(); iter != this->fv_end(); ++iter)
		{
			m_polygon.append_vertex( (*iter)->m_coord);
			mv_halfedge.push_back((HalfEdgeType *) (iter.get_he()));
		}
	}
	
	/**
	 *
	 */
	HalfEdgeType * get_he_by_id (unsigned idx) const 
	{
		unsigned count = 0; 		
		for (FVIter iter = this->fv_begin(); iter != this->fv_end(); ++iter, ++count)
		{
			if (count == idx) return (HalfEdgeType *)(iter.get_he());
		}
		throw ("Invalid HalfEdgeIdx");
		return (HalfEdgeType * )NULL;
	}
	/**
	 * Translate the polgyon idx to halfedge. 
	 */
	HalfEdgeType * polygon_edge_idx2halfedge(unsigned i, unsigned j) const 
	{
		HalfEdgeType * source = this->mv_halfedge[i]; 
		HalfEdgeType * target = this->mv_halfedge[j]; 

		if (source->next == target) 
		{
			return source ; 
		}
		else if (target->next == source)
		{
			return target ;
		}
		else
		{
			throw ("Invalid halfedge index.");
		}
	}


	/**
	 * Predicate the intersection of two different faces. 
	 */
	friend inline bool predicate_face_face (const FaceType * _face1, const FaceType * _face2)
	{
		return predicate_polygon_polygon(_face1->get_polygon(), _face2->get_polygon());
	}

	/**
	 * Manage the co-planar faces. 
	 */
	friend bool intersect_face_face_coplanar (const FaceType * _face1, const FaceType * _face2, BooleanMeshIntersectResult<Value> & _intr)
	{
		//std::vector<std::vector<Vector<Value> > > cut_line; 
		std::vector<pair <unsigned, unsigned > > hepair_idx ;
		std::vector<pair <unsigned, unsigned > > opposite_hepair_idx ;
		bool flag = intersect_polygon_polygon_coplanar (_face1->get_polygon(), _face2->get_polygon(), _intr, _intr.cut_set, _intr.opposite_cut_set, hepair_idx, opposite_hepair_idx);


		//std::cout<<"Cut Lines"<<std::endl;
		for (unsigned i = 0; i < _intr.cut_set.size(); ++i)
		{
			_intr.he_set.push_back(std::make_pair (_face1->get_he_by_id(hepair_idx[i].first ), _face1->get_he_by_id(hepair_idx[i].second )) );
			/*
			for (unsigned j = 0; j < _intr.cut_set[i].size(); ++j)
			{
				std::cout<<_intr.cut_set[i][j] <<' ';
			}

			std::cout<<" -- ";
			std::cout<< hepair_idx[i].first <<' '
					 << hepair_idx[i].second <<std::endl;
			std::cout<< _face1->get_he_by_id(hepair_idx[i].first ) <<' '
					 << _face1->get_he_by_id(hepair_idx[i].second ) <<std::endl;
			std::cout<<std::endl;
			*/
		}
	
		//std::cout<<"Opposite Cut Lines"<<std::endl;
		for (unsigned i = 0; i < _intr.opposite_cut_set.size(); ++i)
		{
			_intr.opposite_he_set.push_back(std::make_pair (_face2->get_he_by_id(opposite_hepair_idx[i].first ), _face2->get_he_by_id(opposite_hepair_idx[i].second ) ) );
			/*
			for (unsigned j = 0; j < _intr.opposite_cut_set[i].size(); ++j)
			{
				std::cout<<_intr.opposite_cut_set[i][j] <<' ';
			}

			std::cout<<" -- ";
			std::cout<< opposite_hepair_idx[i].first <<' '
					 << opposite_hepair_idx[i].second <<std::endl;
			std::cout<< _face2->get_he_by_id(opposite_hepair_idx[i].first ) <<' '
					 << _face2->get_he_by_id(opposite_hepair_idx[i].second ) <<std::endl;
			std::cout<<std::endl;
			*/
		}
	
		return flag; 
	}	

	friend void make_merge_key (const FaceType * _face1, const FaceType * _face2, std::pair <unsigned, unsigned> & key)
	{
		if (_face1->m_idx < _face2->m_idx) 
		{
			key.first = _face1->m_idx; key.second = _face2->m_idx; 
		}
		else
		{
			key.first = _face2->m_idx; key.second = _face1->m_idx; 
		}
	}
	/**
	 * Add the intersection result into the merge structure 
	 */
	friend void add_merge_intersection_first (const FaceType * _face1, const FaceType * _face2, BooleanMeshIntersectResult<Value> & _intr)
	{
		typename std::map <std::pair <unsigned, unsigned>, IntersectionRecord<Value> >::iterator mapIter;
		std::pair<unsigned , unsigned> key; 
		make_merge_key (_face1, _face2, key);
		IntersectionRecord <ValueType> ir ; 
		if (map_mergeInter.end() == map_mergeInter.find (key))
		{
			switch(_intr.res_type )
			{
			case SEGMENT :
				ir.type = SEGMENT ;
				ir.segment = _intr.segment ;
				// Add the halfedge 
				break; 
				;
			case POLYGON :
				ir.type = POLYGON ;
				ir.polygon = _intr.polygon ;
				break; 
				;
			default:
				throw ("Unhandled intersection result");
				;
			}
			map_mergeInter.insert(std::make_pair (key, ir));
			
		   
		}
		else
		{
		}
		//= map_mergeInter.find ();
	}
    /**
	 * Add the intersection result into the merge structure 
	 */
	friend void add_merge_intersection_second ()
	{
	}
	/**
	 * We have to rewrite the intersect part to record the face topology info. 
	 */
	friend bool intersect_face_face (const FaceType * _face1, const FaceType * _face2, BooleanMeshIntersectResult<Value> & _intr, bool & iscoplanar)
	{
		iscoplanar = false; 

		const FaceType & _pol1 = * _face1;
		const FaceType & _pol2 = * _face2;

		Vector<Value> normal1 = _pol1.normal();
		Vector<Value> normal2 = _pol2.normal();

		//std::cout<<_pol1.get_polygon();
		//std::cout<<_pol2.get_polygon(); 

		unsigned size1 = _pol1.size();
		unsigned size2 = _pol2.size(); 
		unsigned u0, u1, u2, u3;
		u0 =  u1 =  u2 = u3 = size1; 

		/*
		Value td0 = orient_3d (_pol1[0], _pol1[1], _pol1[2], _pol2[0]);
		Value td1 = orient_3d (_pol2[0], _pol2[1], _pol2[2], _pol1[0]);

		if (abs (td0 * td1) < tiny_value )
		{
			iscoplanar = true; 
			return intersect_face_face_coplanar(_face1, _face2, _intr);
		}
		*/

		unsigned coplanar1 = 0;

		unsigned i = 0, j = size1 - 1; 
		for (; i < size1; j = i++)
		{
			Value d0 = (_pol1[j] - _pol2[0]) * normal2 ; // Predicate
			Value d1 = (_pol1[i] - _pol2[0]) * normal2 ; // Predicate
			if (abs (d1) < Value(tiny_value)) coplanar1 ++ ;
			if (d0 * d1 <= Value(tiny_value))
			{
				if (d0 >= Value(tiny_value) && d1 < -Value(tiny_value)) { u0 = j ; u1 = i; }
				else if (d0 > Value(tiny_value) && d1 <= -Value(tiny_value)) { u0 = j ; u1 = i; }
				else if (d1 >= Value(tiny_value) && d0 < -Value(tiny_value)){ u3 = i ; u2 = j; }
				else if (d1 > Value(tiny_value) && d0 <= -Value(tiny_value)){ u3 = i ; u2 = j; }
			
			}
			
		}

		if (coplanar1 >= 3) 
		{
			//throw ("Co-planar polygons.");
			iscoplanar = true; 
			return intersect_face_face_coplanar(_face1, _face2, _intr);
		}
/*
		else if (coplanar == 2) 
		{
			throw ("Edge face intersection."); 
		}
*/
		if ( u0 == size1 || u2 == size1 ) return false;  
	
		unsigned v0, v1, v2, v3 ;
		v0 = v1 = v2 = v3 = size2; 
		unsigned coplanar2 = 0;
		for (i = 0, j = size2 - 1; i < size2; j = i++)
		{
			Value d0 = ( _pol2[j] - _pol1[0] ) * normal1 ; // Predicate
			Value d1 = ( _pol2[i] - _pol1[0] ) * normal1 ; // Predicate
			if (abs (d1) < Value(tiny_value)) coplanar2 ++ ;
			if (d0 * d1 <= Value(tiny_value))
			{
				if (d0 >= Value(tiny_value) && d1 < -Value(tiny_value)) { v0 = j ; v1 = i; }
				else if (d0 > Value(tiny_value) && d1 <= -Value(tiny_value)) { v0 = j ; v1 = i; }
				else if (d1 >= Value(tiny_value) && d0 < -Value(tiny_value)){ v3 = i ; v2 = j; }
				else if (d1 > Value(tiny_value) && d0 <= -Value(tiny_value)){ v3 = i ; v2 = j; }
			}
			/*
			if (d0 * d1 <= Value(0))
			{
				if (d0 >= Value(0) && d1 < Value(0)) { v0 = j ; v1 = i; }
				else if (d0 > Value(0) && d1 <= Value(0)) { v0 = j ; v1 = i; }
				else if (d1 >= Value(0) && d0 < Value(0)){ v3 = i ; v2 = j; }
				else if (d1 > Value(0) && d0 <= Value(0)){ v3 = i ; v2 = j; }
			}
			*/
		}
		if (v0 == size2 || v2 == size2 ) return false; 

		Value d0 = orient_3d (_pol1[u0], _pol1[u1], _pol2[v0], _pol2[v1]);
		Value d1 = orient_3d (_pol1[u2], _pol1[u3], _pol2[v3], _pol2[v2]);
	
		if (!(d0 <= tiny_value && d1 <= tiny_value)) return false;

		Value d2 = orient_3d (_pol1[u0], _pol1[u1], _pol2[v3], _pol2[v2]);
		Value d3 = orient_3d (_pol1[u2], _pol1[u3], _pol2[v0], _pol2[v1]);

		// Construct the intersection here. 
		std::cout<<"coplanar "<<coplanar1<<' '<<coplanar2<<std::endl;
		Vector<Value> source, target; 
		unsigned u, v; 
		HalfEdgeType * he; 

		// The intersection part is a point. 
		//if (d0 == 0 || d1 == 0)
		if ((d0 > -tiny_value && d0 < tiny_value ) || (d1 > -tiny_value && d1 < tiny_value ))
		{
			std::cout<<"POINT Degenerated cases."<<std::endl;
			//throw ("POINT Degenerated cases. ");
			//std::cout << _intr.v_he[_intr.v_he.size() - 1]->face->m_idx << std::endl;
			if (d2 < Value(0))
			{
				if (u1 == u2) u = u3; else u = u2; 
				source = intersect_seg_triangle (_pol2[v3], _pol2[v2], _pol1[u0], _pol1[u1], _pol1[u]); 
			}
			else 
			{
				if (v1 == v2) v = v3; else v = v2;
				source = intersect_seg_triangle (_pol1[u0], _pol1[u1], _pol2[v0], _pol2[v1], _pol2[v]);
			}
			_intr.res_type = POINT ;
			_intr.point = source ;
			return true; 
		}


		if (d2 < -Value(tiny_value))
		{
			if (u1 == u2) u = u3; else u = u2; 
			source = intersect_seg_triangle (_pol2[v3], _pol2[v2], _pol1[u0], _pol1[u1], _pol1[u]); 
			he = _face2->polygon_edge_idx2halfedge(v3, v2); 
			_intr.source_he = he; 
		}
		else 
		{
			if (v1 == v2) v = v3; else v = v2;
			source = intersect_seg_triangle (_pol1[u0], _pol1[u1], _pol2[v0], _pol2[v1], _pol2[v]);
			he = _face1->polygon_edge_idx2halfedge(u0, u1);
			_intr.source_he = he; 
		}


		if (d3 < -Value(tiny_value))
		{
			if (u1 == u2) u = u3;  else u = u2; 
			target = intersect_seg_triangle (_pol2[v0], _pol2[v1], _pol1[u0], _pol1[u1], _pol1[u]);
			he = _face2->polygon_edge_idx2halfedge(v0, v1);
			_intr.target_he = he; 
		}
		else
		{
			if (v1 == v2) v = v3; else v = v2; 
			target = intersect_seg_triangle (_pol1[u2], _pol1[u3], _pol2[v0], _pol2[v1], _pol2[v]);
			he = _face1->polygon_edge_idx2halfedge(u2, u3);
			_intr.target_he = he; 
		}

		if ((source - target).square_module()< sq_tiny_value) 
		{
			_intr.res_type = POINT ;
			_intr.point = source; 
			return true; 
			//throw (" Point . Degeneracy cases. ");
		}

		// The intersection part is a segment. 
		_intr.res_type = SEGMENT ;
		_intr.segment = Segment<Value>(source, target);

		/*
		if (coplanar1 != 2 && coplanar2 == 2)
		{
			return true ;
		}
		else return false; 
		*/
		return true; 
	}


	/**
	 * This function will be called when updating data of mesh. 
	 */
	void update ()
	{
		ParentType::update();
		m_polygon.clear();
		for (FVIter iter = this->fv_begin(); iter != this->fv_end(); ++iter)
		{
			m_polygon.append_vertex( (*iter)->m_coord);
		}
	}
protected :
	PolygonType m_polygon ;
	std::vector<HalfEdgeType *> mv_halfedge;  

public :
	bool accessed ;

public :
//	bool splitted ; // True : has been splitted, so cannot be copied; False : otherwise; 
};


template <typename Value>
class FaceGraph 
{


protected:
	typedef typename BooleanMeshPrimitive<Value>::ValueType    ValueType; 
	typedef typename BooleanMeshPrimitive<Value>::FaceType     FaceType;
	typedef typename BooleanMeshPrimitive<Value>::VertexType   VertexType;
	typedef typename BooleanMeshPrimitive<Value>::HalfEdgeType HalfEdgeType ; 
	typedef typename BooleanMeshPrimitive<Value>::EdgeType     EdgeType ; 
	typedef typename BooleanMeshPrimitive<Value>::VectorType   VectorType ;
	typedef typename BooleanMeshPrimitive<Value>::PointType    PointType ;
	typedef typename BooleanMeshPrimitive<Value>::MeshType    MeshType ;


	typedef AABBTreePoly <ValueType>                         AABBTreePolyType; 
	typedef AABBTreePolyBuilder <ValueType>                  AABBTreePolyBuilderType  ; 
	typedef AABBTreePolygon <ValueType>                      AABBTreePolygonType ;
	typedef AABBTreeNode <ValueType> AABBTreeNodeType ;
	typedef Box<ValueType> BoxType; 
    typedef Ray<ValueType>                                        RayType ;


    typedef typename AABBTreePolyType::PolygonColResult           PolygonColResult;
    typedef typename AABBTreePolyType::RayColResult               RayColResult;

   
	typedef Segment <Value>                                    SegmentType ;
	typedef typename FaceType::FHeIter                         FHeIter; 
	typedef Face<Value>                                        ParentType ;
	typedef Polygon<Value>                                     PolygonType ;

	typedef typename FaceType::FVIter                          FVIter; 

	enum PTPOS {EDGE, VERTEX, INSIDE, OUTSIDE};
	struct PTInfo 
	{
		PointType coord;
		PTPOS ptPos; 
		unsigned vidx; 
	};
	// For new face classification. 
	enum FACETYPE {IN = 0x0001, ON = 0x0002, OUT = 0x0004};
private :
	PTInfo & fill_ptinfo (PTInfo & ptInfo, const PointType & _coord, const PTPOS _ptPos, unsigned idx)
	{
		ptInfo.coord = _coord; ptInfo.ptPos = _ptPos; ptInfo.vidx = idx; 
		return ptInfo; 
	}
public :
	FaceGraph (MeshType * m1, MeshType * m2, FaceType * face, unsigned max_idx) : this_mesh (m1), mesh2(m2), mp_face(face), m_max_idx(max_idx)
	{
		// Add vertices.  
		std::vector <PointType * > vp; 
		std::vector <int> vid; 
		mp_face->Face2VertexCoord (vp);
		mp_face->Face2VertexIdx (vid);
		for (unsigned i = 0; i < vp.size(); ++i)
		{
			PTInfo ptInfo; 
			v_point_set.push_back (fill_ptinfo (ptInfo, *vp[i], VERTEX, vid[i]));
		}
		// Add existing edge points. 
		HalfEdgeType * he = (HalfEdgeType * ) face->he; 
		HalfEdgeType * end = he;
		do 
		{
			EdgeType * edge = (EdgeType *) he->edge;
			for (unsigned i = 0; i < edge->v_point.size(); ++i)
			{
				PTInfo ptInfo; 
				v_point_set.push_back (fill_ptinfo (ptInfo, edge->v_point[i].first, EDGE, edge->v_point[i].second));
			}
			he = (HalfEdgeType *)he->next ;
		} while (he != end);
		// Pending. 
	}

	bool pt_equal (const PointType & pt1, const PointType & pt2) const
	{
		return (pt1 - pt2 ).square_module() <= tiny_value ;
	} 
	unsigned add_edge_point (const PointType & p, unsigned edge_count)
	{
		HalfEdgeType * he = (HalfEdgeType *)this->mp_face->get_he_by_id(edge_count);
		EdgeType * edge = (EdgeType *)he->edge; 
		unsigned size = edge->v_point.size();
		for (unsigned i = 0; i < size; ++i)
		{
			if (pt_equal (edge->v_point[i].first, p))
			{
				return edge->v_point[i].second; 
			}
		}
		edge->v_point.push_back (std::make_pair (p, m_max_idx));
		m_max_idx ++ ;
		return (m_max_idx - 1) ;
	}
	unsigned translate_coord_index (const PointType & p)
	{
		//Check if it is an existing vertex. 
		for (unsigned i = 0; i < v_point_set.size(); ++i)
		{
			if (pt_equal (v_point_set[i].coord , p))
			{
				return v_point_set[i].vidx; 
			}
		}

		// Try to add edge vertex
		PTInfo ptInfo; 
		const PolygonType & pol  = mp_face->get_polygon (); 
		unsigned edge_count; 
		PTPOS pos = INSIDE ;
		if (is_on_edge_3d (pol, p, edge_count)) 
		{
			pos = EDGE ;
			unsigned vidx = add_edge_point (p, edge_count);
			v_point_set.push_back (fill_ptinfo (ptInfo, p, pos, vidx));
			return vidx; 
		}
		
		// Add it as a face point 
		v_point_set.push_back (fill_ptinfo (ptInfo, p, pos, m_max_idx));
		++m_max_idx;
		return (m_max_idx - 1); 
	}
	// Add the segment not on the boundary
	void add_inside_segment (const SegmentType & seg)
	{
		const PointType & p0 = seg.p0 ;
		const PointType & p1 = seg.p1 ;
		unsigned id1 = translate_coord_index (p0);
		unsigned id2 = translate_coord_index (p1);
		// check if they are on the edges.
		// Pending. 
		std::cout<<'('<<id1<<','<< id2<<')'<<std::endl;
		add_into_graph(id1, id2); 
		add_into_graph(id2, id1); 
		// build graph. 
		
	}
	// Sort the point on edges
	void sort_edge_point (HalfEdgeType * he) 
	{
		std::vector<std::pair<PointType, unsigned> > & pt_v = ((EdgeType * )he->edge)->v_point ;
		const PointType & ref_point = he->from_v->m_coord;
		unsigned size = pt_v.size(); 
		for (unsigned iPos = 0; iPos < size; iPos++)
		{
			unsigned iMin = iPos;
			for (unsigned i = iPos+1; i < size; i++)
			{
				if ((pt_v[i].first - ref_point).square_module() < (pt_v[iMin].first - ref_point).square_module())
				{
						iMin = i;
				}
			}

			if ( iMin != iPos )
			{
				std::swap(pt_v[iPos], pt_v[iMin]);
			}
		}
	}

	void split_edge (HalfEdgeType * he)
	{
		EdgeType * edge = (EdgeType * )he->edge; 
		if (edge->v_point.size() == 0)
		{
			unsigned id1 = he->from_v->m_idx ;
			unsigned id2 = he->next->from_v->m_idx ;
			std::cout<< '('<<id1<<','<<id2<<')'<<std::endl;	
			add_into_graph(id1, id2); 
			add_into_graph(id2, id1); 
			return ;
		}

		// else
		unsigned id1 = he->from_v->m_idx;
		unsigned id2 = 0; 
		for (unsigned i = 0; i < edge->v_point.size(); ++i)
		{
			id2 = edge->v_point[i].second ; 
			std::cout<< '('<<id1<<','<<id2<<')'<<std::endl;	
			add_into_graph(id1, id2);
			add_into_graph(id2, id1); 
			id1 = id2; 
		}
		id2 = he->next->from_v->m_idx ;
		std::cout<< '('<<id1<<','<<id2<<')'<<std::endl;	
		add_into_graph(id1, id2); 
		add_into_graph(id2, id1); 
	}
	// Add the segment on the boundary
	void add_boundary_segment ()
	{
		for (FVIter iter = this->mp_face->fv_begin(); iter != this->mp_face->fv_end(); ++iter)
		{
			//if (count == idx) return (HalfEdgeType *)(iter.get_he());
			HalfEdgeType * he = (HalfEdgeType *)(iter.get_he()) ;
			sort_edge_point (he);
			split_edge (he);
		}
	}
	unsigned get_max_id () const {return m_max_idx ;}

	void add_into_graph (unsigned id1, unsigned id2)
	{
		typename std::map <unsigned, set <unsigned> >::iterator iter; 
		iter = m_graph_unsorted.find (id1);
		if (iter == m_graph_unsorted.end())
		{
			std::set<unsigned> set2; 
			set2.insert (id2);
			this->m_graph_unsorted.insert (std::make_pair (id1, set2));
			return ;
		}
		else
		{
			iter->second.insert (id2);
		}
	}

	struct AccRecord
	{
		unsigned vid; 
		bool acc ; 
	};


	ValueType compare (const AccRecord & _p1, const AccRecord & _p2, unsigned ref, const VectorType & normal ) 
	{
		unsigned id1 = _p1.vid; 
		unsigned id2 = _p2.vid; 
//		std::cout<<"compare "<<id1<<' '<<id2<<' ' ;

		PointType pref = map_id_point.find (ref)->second.coord;
		PointType p1 = map_id_point.find (id1)->second.coord ;
		PointType p2 = map_id_point.find (id2)->second.coord ;

		return (orient_3d (pref, p1, p2, normal + pref) );
	}

	void split_neighbor_point (const std::vector<AccRecord> & neighbor, unsigned ref, const VectorType & normal, std::vector<AccRecord> & positive, std::vector<AccRecord> & nonpositive)
	{
		unsigned start_idx = 0; 
		positive.push_back(neighbor[start_idx]);
		for (unsigned i = 1; i < neighbor.size(); ++i)
		{
			if (compare (neighbor[i], neighbor[start_idx], ref, normal) > 0)
				positive.push_back(neighbor[i]);
			else
				nonpositive.push_back(neighbor[i]);
		}
		
	}

	void sort_point (std::vector<AccRecord> & neighbor, unsigned ref, const VectorType & normal ) 
	{		
		unsigned size = neighbor.size(); 
		for (unsigned iPos = 0; iPos < size; iPos++)
		{
			unsigned iMin = iPos;
			for (unsigned i = iPos+1; i < size; i++)
				if (compare (neighbor [i], neighbor[iMin], ref, normal) > 0) iMin = i;
			if ( iMin != iPos )
				std::swap(neighbor [iPos], neighbor[iMin]);
		}
	}
	void sort_neighbor_point (std::vector<AccRecord> & neighbor, unsigned ref, const VectorType & normal ) 
	{
		std::vector<AccRecord> positive, nonpositive ;
		split_neighbor_point(neighbor, ref, normal, positive, nonpositive);
		sort_point(positive, ref, normal);
		sort_point(nonpositive, ref, normal);
		neighbor.clear(); 
		neighbor.insert (neighbor.end(), positive.begin(), positive.end());
		neighbor.insert (neighbor.end(), nonpositive.begin(), nonpositive.end());
	}
	/*
	void sort_neighbor_point (std::vector<AccRecord> & neighbor, unsigned ref, const VectorType & normal )
	{
		std::cout<<"Begin sort "<<std::endl;
		std::cout<<ref<<':' ;
		for (unsigned i = 0; i < neighbor.size(); ++i) std::cout<< neighbor[i].vid<<' ' ;
		std::cout<<endl;
		//sort (neighbor.begin(), neighbor.end(), boost::bind (&myclass::compare, this,  boost::lambda::_1, ref) < boost::bind (compare, boost::lambda::_2, ref));
		sort (neighbor.begin(), neighbor.end(), boost::bind (&FaceGraph<ValueType>::compare, this, boost::lambda::_1, boost::lambda::_2, ref, normal));
		std::cout<<"After sort "<<std::endl;
		std::cout<<ref<<':' ;
		for (unsigned i = 0; i < neighbor.size(); ++i) std::cout<< neighbor[i].vid<<' ' ;
		std::cout<<endl;
	}
	*/
	void build_graph ()
	{
		// inverse v_point_set; 
		typename std::vector <PTInfo >::iterator iter; 
		for (iter = v_point_set.begin(); iter!=v_point_set.end(); ++iter )
		{
			map_id_point.insert (std::make_pair ( (*iter).vidx, (*iter))); 
			
			std::cout<<iter->vidx<<' '<<iter->coord<<' ';
			switch (iter->ptPos)
			{
			case VERTEX : std::cout<<"ONVERTEX" ; break;
			case EDGE : std::cout<<"ONEDGE " ; break ;
			case INSIDE : std::cout<<"INSIDE" ; break;
			case OUTSIDE : std::cout<<"OUTSIDE" ; break;
			}
			std::cout<<std::endl;

		}
		
		typename std::map <unsigned, set <unsigned> >::iterator iter1 ;
		typename std::set<unsigned>::iterator iter2; 
		// build sorted graph;
		for (iter1 = m_graph_unsorted.begin(); iter1 != m_graph_unsorted.end(); ++iter1)
		{
			std::vector <AccRecord> v;
			for (iter2 = iter1->second.begin(); iter2 != iter1->second.end(); ++iter2)
			{
				AccRecord acc ; 
				acc.vid = *iter2 ; acc.acc = false; 
				v.push_back(acc);
			}
			// Sort two times to sort the point in a circle. 
			sort_neighbor_point (v, iter1->first, this->mp_face->normal()) ;
			//sort_neighbor_point (v, iter1->first, this->mp_face->normal()) ;
			m_graph_sorted.insert(std::make_pair (iter1->first, v));
		}		

		typename std::map <unsigned, std::vector <AccRecord> >::iterator iter3;
		typename std::vector<AccRecord>::iterator iter4 ;
		for (iter3 = m_graph_sorted.begin(); iter3 != m_graph_sorted.end(); ++iter3)
		{
			std::cout<<iter3->first<<": ";
			for (iter4 = iter3->second.begin(); iter4 != iter3->second.end(); ++iter4)
			{
				std::cout<<(*iter4).vid<<' ';
			}
			std::cout<<std::endl;
		}
	}
    /// auxilibary functions for building faces. 
	bool find_first_vertex (unsigned & index, unsigned & next)
	{
		typename std::map <unsigned, std::vector <AccRecord> >::iterator iter3;
		typename std::vector<AccRecord>::iterator iter4 ;
		for (iter3 = m_graph_sorted.begin(); iter3 != m_graph_sorted.end(); ++iter3)
		{
			/*
			iter4 = iter3->second.begin();
			if (iter4->acc == false) {
				iter4->acc = true; 
				change_acc_record (iter4->vid, iter3->first);
				index = iter3->first ; next = iter4->vid; return true;}
			*/
			VectorType normal = this->mp_face->normal();
			normal.normalize();
			for (iter4 = iter3->second.begin(); iter4 != iter3->second.end(); ++iter4)
			{
				if (iter4->acc == false) 
				{
					// Check if it is circle around the outline
					unsigned id1 = iter3->first; 
					unsigned id2 = iter4->vid; 
					unsigned id3 = find_second_vertex (iter3->first, iter4->vid); 

					PTPOS pos1 = this->map_id_point.find (id1)->second.ptPos; 
					PTPOS pos2 = this->map_id_point.find (id2)->second.ptPos; 
					PTPOS pos3 = this->map_id_point.find (id3)->second.ptPos; 

					PointType p1 = this->map_id_point.find (id1)->second.coord ;
					PointType p2 = this->map_id_point.find (id2)->second.coord ;
					PointType p3 = this->map_id_point.find (id3)->second.coord ;

					// P might be zero , so we cannot normalize it. 
					VectorType p = ((p3 - p2 )^ (p1 - p2)) ; 					
					//VectorType p = ((p1 - p2 )^ (p1 - p3)) ; 
					//p.normalize(); 
					if ((pos1 == VERTEX || pos1 == EDGE ) &&
						(pos2 == VERTEX || pos2 == EDGE ) &&
						(pos3 == VERTEX || pos3 == EDGE ) && 
						 p * normal <= tiny_value 
						) continue; 
					iter4->acc = true; 
					//change_acc_record (iter4->vid, iter3->first);
					index = iter3->first ; next = iter4->vid; return true;
				}
			}
		}		
		return false; 
	}

	unsigned find_second_vertex (unsigned first_vid, unsigned next_vid)
	{
		typename std::map <unsigned, std::vector <AccRecord> >::iterator iter1;
		typename std::vector<AccRecord>::iterator iter2 ;		
		iter1 = m_graph_sorted.find(next_vid);
		if (iter1 == m_graph_sorted.end()) throw ("Error 1");
		unsigned size = iter1->second.size();
		for (unsigned i = 0; i < size; ++i )
		{
			if (iter1->second[i].vid == first_vid)
			{
				unsigned pos = (i + size - 1) % size ;
				return iter1->second[pos].vid;
			}
		}
		throw ("Fail to find next");
		return -1 ;
	}

	void change_acc_record (unsigned id1, unsigned id2)
	{
		typename std::map <unsigned, std::vector <AccRecord> >::iterator iter1;
		typename std::vector<AccRecord>::iterator iter2 ;		
		iter1 = m_graph_sorted.find(id1);
		unsigned size = iter1->second.size();
		for (unsigned i = 0; i < size; ++i )
		{
			if (iter1->second[i].vid == id2)
			{
				iter1->second[i].acc = true; 
				return ;
			}
		}
		throw ("Fail to find edge");
	}
	unsigned find_next_vertex (unsigned first_vid, unsigned next_vid)
	{
		typename std::map <unsigned, std::vector <AccRecord> >::iterator iter1;
		typename std::vector<AccRecord>::iterator iter2 ;		
		iter1 = m_graph_sorted.find(next_vid);
		if (iter1 == m_graph_sorted.end()) throw ("Error 1");
		unsigned size = iter1->second.size();
		for (unsigned i = 0; i < size; ++i )
		{
			if (iter1->second[i].vid == first_vid)
			{
				unsigned pos = (i + size - 1) % size ;
				iter1->second[pos].acc = true; 
				change_acc_record (next_vid, iter1->second[pos].vid);
				//change_acc_record (iter1->second[pos].vid, next_vid);
				return iter1->second[pos].vid;
			}
		}
		throw ("Fail to find next");
		return -1 ;
	}
	bool build_one_face (unsigned first_vid, unsigned next_vid,  std::vector<unsigned> & v_vid, int face_type)
	{
		unsigned current = first_vid; 
		unsigned next ;
		v_vid.push_back(current ); v_vid.push_back(next_vid);
		while ( (next = find_next_vertex (current , next_vid)) != first_vid)
		{
			v_vid.push_back(next ); 
			current = next_vid ;
			next_vid = next ;
		}
		FACETYPE ft = classify_face (v_vid, mp_face->normal());
		for_each (v_vid.begin(), v_vid.end(), std::cout<<boost::lambda::_1<<' ');
		std::cout<<endl;
		if (ft & face_type) return true; 
		else {return false; }
	}

	VertexType * create_new_vertex (MeshType & mesh, const PointType & pt, unsigned vid)
	{
		VertexType * pv = new VertexType ;
		pv->m_coord = pt;
		pv->mp_mesh = & mesh; 
		pv->m_idx =  vid;
		mesh.vertices[vid] = pv; 
		VertexType * tag_vptr = pv;   
		return tag_vptr; 
	}

	void construct_new_face (const std::vector<unsigned> & v_vid, MeshType & mesh )
	{
		for (unsigned i = 0; i < v_vid.size(); ++i)
		{
			if (mesh.vertices.find(v_vid[i]) == mesh.vertices.end())
			{
				typename std::map <unsigned , PTInfo >::iterator iter ; 
				iter = map_id_point.find(v_vid[i]);
				create_new_vertex (mesh, iter->second.coord, v_vid[i]);				
			}
		}
		FaceType * face = mesh.construct_face (v_vid);
		mesh.v_new_face_id.push_back(face->m_idx);
		std::cout<<"Face id "<<face->m_idx<<std::endl;
	}
	void build_face (MeshType & mesh , int face_type_flag)
	{
		unsigned first_vid, next_vid; 
		while (find_first_vertex(first_vid, next_vid))
		{
			std::vector<unsigned>  v_vid;
			if (build_one_face(first_vid, next_vid, v_vid, face_type_flag)) 
				construct_new_face (v_vid, mesh);
			else 
			{
				std::cout<<"classify face fail"<<std::endl;
			}
		}
	}

	///// classify faces 
public :

	FACETYPE is_in_volumn (const PointType & _p, const VectorType & _normal )
	{
		AABBTreePolyType * tree = mesh2->get_tree (); 
		RayType ray (_p, _normal);
		RayColResult rayColRes;
		tree->collideWithRay (ray, rayColRes);	
//		std::cout<<"Query Point Normal"<<_p<<_normal<<std::endl; 
//		std::cout<<"Size "<<rayColRes.polys_.size()<<std::endl;	   
		IntersectResult<ValueType> res; 
		if (rayColRes.polys_.size() > 0)
		{
			unsigned idx = -1; 
			ValueType distance = large_value; 
			for (unsigned i = 0; i < rayColRes.polys_.size(); ++i)
			{  // std::cout<<"Normal"<<rayColRes.polys_[i]->get_face()->normal()<<std::endl;
				const PolygonType & polygon = ((FaceType *) rayColRes.polys_[i]->get_face())->get_polygon(); 
				//std::cout<<polygon<<' '<< ((FaceType *) rayColRes.polys_[i]->get_face())->m_idx<<std::endl;
				intersect_ray_triangle (ray, polygon, res);
				if (res.res_type == POINT)
				{ //std::cout<<"Intersect point "<<res.point<<std::endl;
					ValueType d = (res.point - _p).square_module();
					if ( d < distance)
					{
						idx = i; 
						distance = d ;
					}
				}
				else
				{
					throw ("Fail in find the nearest intersection point");
				}
			}
			if ( abs (distance ) <= tiny_value ) return ON; 
			const AABBTreePolygon<Value> * p = rayColRes.polys_[idx];
			VectorType faceNormal = p->get_face()->normal() ;
			ValueType test = faceNormal * _normal;
			if ( test>= 0 ) return IN; 
		}
		return OUT ;
	}


	PointType find_point_in_face (const std::vector<unsigned>  & v_vid, const VectorType & normal )
	{
		/*
		typename std::map <unsigned , PTInfo >::iterator iter1, iter2, iter3 ; 
		unsigned size = v_vid.size(); 
		for (unsigned i = 0; i < size; ++i)
		{
			iter1 = map_id_point.find (v_vid[i]);
			iter2 = map_id_point.find (v_vid[(i + 1)% size]);
			iter3 = map_id_point.find (v_vid[(i + 2)% size]);
			PointType p1 = iter1->second.coord; 
			PointType p2 = iter2->second.coord; 
			PointType p3 = iter3->second.coord; 
			PointType p4 = p1 + normal ;

			if (orient_3d (p1, p2, p3, p4) > 0)
			{
				return (p1 + p2 + p3 + p2) * ValueType(0.25);
				//return (( p1 + p2 ) * ValueType(0.5) + (p3 + p2) * ValueType(0.5)) * ValueType (0.5); 
			}
		}
		throw ("Fail to find inside point");
		return PointType(0, 0, 0);
		*/
		
		
		typename std::map <unsigned , PTInfo >::iterator iter;
		PolygonType pol; 
		unsigned size = v_vid.size(); 
		for (unsigned i = 0; i < size; ++i)
		{
			iter = map_id_point.find(v_vid[i]);
			pol.append_vertex (iter->second.coord);
		}
		return point_in_polygon_3d(pol, normal);
		
	}
	FACETYPE classify_face (const std::vector<unsigned>  & v_vid, const VectorType & normal )
	{
		PointType test_point = find_point_in_face (v_vid, normal);
		std::cout<<"Test Point "<<test_point<<std::endl;
		return is_in_volumn(test_point, normal);
		//if (is_in_volumn(test_point, normal)) return IN; 
		//else return OUT; 
	}
private:
	MeshType * this_mesh; 
	MeshType * mesh2 ;
	FaceType * mp_face; 
	unsigned m_max_idx ;
	std::vector < PTInfo > v_point_set; // map_point coordinates to the point index ;

	std::map <unsigned , PTInfo > map_id_point; // map id to point 

	std::map <unsigned, std::set <unsigned> > m_graph_unsorted ; // build link list graph
	std::map <unsigned, std::vector <AccRecord> > m_graph_sorted; // build sorted link list graph
	
};



#endif
